% clc
% clear

nVar  = 6;

syms sqrtGL sqrtGhL sqrtGvL iGL iGhL iGvL
syms sqrtGR sqrtGhR sqrtGvR iGR iGhR iGvR

qL   = sym('qL'  ,[nVar 1]);
qR   = sym('qR'  ,[nVar 1]);
iGL  = sym('iGL' ,[3,3]);
iGR  = sym('iGR' ,[3,3]);
iGhL = sym('iGhL',[3,3]);
iGhR = sym('iGhR',[3,3]);
iGvL = sym('iGvL',[3,3]);
iGvR = sym('iGvR',[3,3]);

[F,pL,pR] = AUSM(sqrtGL,sqrtGhL,sqrtGvL,iGL,iGvL,qL,...
                 sqrtGR,sqrtGhR,sqrtGvR,iGR,iGvR,qR);

for iVar = 1:nVar
    for jVar = 1:nVar
      dFdqL(iVar,jVar) = diff(F(iVar),qL(jVar));
      dFdqR(iVar,jVar) = diff(F(iVar),qR(jVar));
%       for kVar = 1:nVar
%           d2FdqLdqL(iVar,jVar,kVar) = diff(dFdqL(iVar,jVar),qL(kVar));
%           d2FdqLdqR(iVar,jVar,kVar) = diff(dFdqL(iVar,jVar),qR(kVar));
%           d2FdqRdqL(iVar,jVar,kVar) = diff(dFdqR(iVar,jVar),qL(kVar));
%           d2FdqRdqR(iVar,jVar,kVar) = diff(dFdqR(iVar,jVar),qR(kVar));
%       end
    end
    dpLdqL(iVar) = diff(pL,qL(iVar));
    dpRdqR(iVar) = diff(pR,qR(iVar));
end

% % Check result
% ppp1=subs(dFdqL,[qL(1),qL(2),qL(3),qL(4),qL(5),...
%              qR(1),qR(2),qR(3),qR(4),qR(5),...
%              sqrtGL,sqrtGR,G13L,G13R,...
%              'p0','cpd','cpv','cvd','cvv','Rd','eqv','alph','bet','Ku','Kp','sigm'],...
%     [194.235335018045,-985.256400718011,0,56287.8510036657,0,...
%      194.235335018045,-985.256400718011,0,56287.8510036657,0,...
%      159.283613532977,159.283613532977,0,0,...
%      100000,1004.5,1846.,717.5,1385,287,0.607768516503768,3/16,0.125,0.75,0.25,1]);
% ppp1=double(ppp1);
% 
% ppp2=subs(dFdqL,[qL(1),qL(2),qL(3),qL(4),qL(5),...
%              qR(1),qR(2),qR(3),qR(4),qR(5),...
%              sqrtGL,sqrtGR,G13L,G13R,...
%              'p0','cpd','cpv','cvd','cvv','Rd','eqv','alph','bet','Ku','Kp','sigm'],...
%     [214.655360294857,-1121.66215120058,0,62264.6473510797,0,...
%      214.655360294857,-1121.66215120058,0,62264.6473510797,0,...
%      178.687168517986,178.687168517986,0,0,...
%      100000,1004.5,1846.,717.5,1385,287,0.607768516503768,3/16,0.125,0.75,0.25,1]);
% ppp2=double(ppp2);

% ccode([dpLdqL;dpRdqR],'file','dpdq_jab.c');
% ccode(dpLdqL,'file','dpdq_jab.c');
ccode([dFdqL;dFdqR],'file','AUSM_jab.c');
% ccode([d2FdqLdqL;d2FdqLdqR;d2FdqRdqL;d2FdqRdqR],'file','AUSM_jab2.c');

function [F,pL,pR] = AUSM(sqrtGL,sqrtGhL,sqrtGvL,iGL,iGvL,qL,...
                          sqrtGR,sqrtGhR,sqrtGvR,iGR,iGvR,qR)
syms Ku Kp sigm

sqrtGrhoL = qL(1) + qL(6);
sqrtGrhoR = qR(1) + qR(6);
    
rhoL = sqrtGrhoL / sqrtGL;
rhoR = sqrtGrhoR / sqrtGR;
rho   = 0.5 * ( rhoL + rhoR );

pL  = calc_pressure(sqrtGL,qL);
pR  = calc_pressure(sqrtGR,qR);

uL  = ( qL(4) + sqrtGvL .* ( iGvL(1,3) .* qL(2) + iGvL(2,3) .* qL(3) ) ) ./ sqrtGrhoL;
uR  = ( qR(4) + sqrtGvR .* ( iGvR(1,3) .* qR(2) + iGvR(2,3) .* qR(3) ) ) ./ sqrtGrhoR;

mrL = qL(6) / qL(1);
mrR = qR(6) / qR(1);
    
cL  = calc_sound_speed_nh(rhoL, pL, mrL);
cR  = calc_sound_speed_nh(rhoR, pR, mrR);
a   = 0.5 * (cL + cR);

ML  = uL ./ a;
MR  = uR ./ a;

Mbar2 = ( uL.^2 + uR.^2 ) / ( 2 * a.^2 );
s = sig(1. - sigm * Mbar2, 0.);
mx = s * (1. - sigm * Mbar2);

Ma = M4(ML, 1) + M4(MR, -1) - Kp * mx * (pR - pL) ./ ( rho * a.^2);
m = 0.5 * ( cL ./ sqrtGvL + cR ./ sqrtGvR ) * Ma;

coefL(2) = sqrtGL .* iGL(1,3);
coefR(2) = sqrtGR .* iGR(1,3);
coefL(3) = sqrtGL .* iGL(2,3);
coefR(3) = sqrtGR .* iGR(2,3);
coefL(4) = sqrtGhL;
coefR(4) = sqrtGhR;

p5p = P5(ML,  1);
p5n = P5(MR, -1);
for iVar = 2:4
    P(iVar,1) = p5p .* coefL(iVar) .* pL + p5n .* coefR(iVar) .* pR - Ku .* p5p .* p5n .* ( coefL(iVar) * rhoL + coefR(iVar) .* rhoR ) .* a .* (uR - uL);
end
P(1,1) = 0;
P(5,1) = 0;
P(6,1) = 0;

F = m .* ( sig(m,0) .* qL + ( 1 - sig(m,0) ) .* qR ) + P;

end

function m = P5(M,signal)
syms alph

s = sig(M,0);
sm = s - ( 1 - s );

P51 = 0.5 * ( 1. + signal * sm );
P52 = M2( M, signal ) * ( ( 2. * signal - M ) - signal * 16.*alph * M * M2( M, -signal ) );

s = sig(abs(M),1);

m = s * P51 + (1-s) * P52;

end

function m = M4(M,signal)
syms bet

M41 = 0.5 * ( M + signal * abs(M) );
M42 = M2( M, signal ) * ( 1. - signal * 16. * bet * M2( M, -signal ) );

s = sig(abs(M),1);

m = s * M41 + (1-s) * M42;

end

function m = M2(M,signal)
m = signal * 0.25 * ( M + signal ).^2;
end

function p = calc_pressure(sqrtG,q)
syms p0 Rd cpd cpv cvd cvv eqv
w1 = q(1);
w5 = q(5);
w6 = q(6);

rho      = ( w1 + w6 ) / sqrtG;
gamma    = w6 / w1;
sh       = gamma / ( 1. + gamma );
R        = ( 1. + eqv * sh ) * Rd;
theta    = w5 / w1;
cp       = cpd + ( cpv - cpd ) * sh;
cv       = cvd + ( cvv - cvd ) * sh;
kappa    = cp / cv;

p = p0 * ( rho * theta * R / p0 ).^kappa;
end

function [c] = calc_sound_speed_nh(rho, p, mr)
syms p0 Rd cpd cpv cvd cvv eqv

kappa = ( cpd + mr * cpv ) / ( cvd + mr * cvv );

c = sqrt( kappa * p / rho );

end

function s = sig(x,y)
K = 1.e-15;

s = 0.5 * ( -( 1 + K ) * ( x - y ) / ( -abs( x - y ) - K ) + 1 );

end